// --------------------------------------------------------------------------------------------------------------------
// <copyright file="JointControllerDisplayAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Manipulation.Visualization
{
    using System;
    using System.Collections;
    using System.Collections.Generic;
    using System.Collections.ObjectModel;
    using System.IO;
    using System.Runtime.Serialization;
    using System.Text;
    using System.Xml;
    using Microsoft.Robotics.Manipulation;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.Arm;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Visualization;

    /// <summary>
    /// This model agent class prepares visualization for arm manipulation.
    /// </summary>
    [DataContract]
    public sealed class JointControllerDisplayAgent : ModelAgent<ManipulatorStatusMessage, JointControllerDisplayModel>
    {
        /// <summary>
        /// Name of the manipulation agent
        /// </summary>
        [DataMember(Name = "ArmControlAgentName")]
        private string armControlAgentName = default(string);

        /// <summary>
        /// Number of joints.
        /// </summary>
        [DataMember(Name = "NumJoints")]
        private int numJoints;

        /// <summary>
        /// Control port for arm cartesian control
        /// </summary>
        private IForwarder<JointGoalPositionControlMessage> armJointControlPort;

        /// <summary>
        /// Control port for setting PID gains.
        /// </summary>
        private IForwarder<SetJointVelocityPIDGainsMessage> setPidGainsPort;

        /// <summary>
        /// Current joint targets.  Used to determine if models targets have changed.
        /// </summary>
        private double[] currentJointTargets;

        /// <summary>
        /// Initializes a new instance of the <see cref="JointControllerDisplayAgent"/> class.
        /// </summary>
        /// <param name="name">Agent name</param>
        /// <param name="producer">The name of the agent we subscribe to.</param>
        /// <param name="maxUpdateIntervalMilliseconds">The interval on which to issue data binding updates to the UI</param>
        /// <param name="armControlAgentName"> Name of the agent controlling the arm.</param>
        /// <param name="numJoints">Number of joints.</param>
        public JointControllerDisplayAgent(
            string name, 
            IProducer<ManipulatorStatusMessage> producer,
            int maxUpdateIntervalMilliseconds,
            string armControlAgentName,
            int numJoints)
            : base(name, producer.Name, maxUpdateIntervalMilliseconds)
        {
            this.armControlAgentName = armControlAgentName;
            this.numJoints = numJoints;
        }

        /// <summary>
        /// Initializes any fields not provided in the manifest.
        /// </summary>
        /// <param name="locator">Not used.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.currentJointTargets = new double[this.numJoints];

            for (int i = 0; i < this.currentJointTargets.Length; ++i)
            {
                this.currentJointTargets[i] = double.NaN;
            }

            IAgentHost manipulationStatusProducerAgent = this.ProducerAgent;

            manipulationStatusProducerAgent.Subscribe<ManipulatorStatusMessage>(this);

            IAgentHost manipulatorAgent = locator.GetAgent(this.armControlAgentName);

            // arm control agent not optional for this display, so allow any exceptions to percolate up here
            this.armJointControlPort = manipulatorAgent.GetControlPort<JointGoalPositionControlMessage>();
            this.setPidGainsPort = manipulatorAgent.GetControlPort<SetJointVelocityPIDGainsMessage>();
        }

        /// <summary>
        /// Updates the model data with the new state.
        /// </summary>
        /// <param name="message">The new state from the upstream agent.</param>
        /// <returns>true if the model was updated, false otherwise</returns>
        protected override bool UpdateModel(ManipulatorStatusMessage message)
        {
            ManipulatorStatus state = message.State;

            if (double.IsNaN(this.currentJointTargets[0]))
            {
                Array.Copy(state.JointPositions, this.currentJointTargets, state.JointPositions.Length);
            }

            Array.Copy(state.JointPositions, Model.JointActualPosition, state.JointPositions.Length);

            int index;
            while (this.Model.PDGainsToUpdate.Count > 0)
            {
                if (!this.Model.PDGainsToUpdate.TryDequeue(out index))
                {
                    break;
                }

                SetJointVelocityPIDGainsMessage pidMessage = new SetJointVelocityPIDGainsMessage(
                    DateTime.Now.Ticks,
                    Model.PGains[index],
                    0,
                    Model.DGains[index],
                    index,
                    Model.Deadband[index],
                    Model.MaxDelta[index]);

                this.setPidGainsPort.Post(pidMessage);
            }

            if (this.Model.EnablePositionCommands)
            {
                if (this.JointTargetsHaveChanged())
                {
                    Array.Copy(Model.JointCommandedPosition, this.currentJointTargets, this.numJoints);

                    JointGoalPositionControlMessage controlMessage = new JointGoalPositionControlMessage(
                        DateTime.Now.Ticks,
                        this.currentJointTargets,
                        0);

                    this.armJointControlPort.Post(controlMessage);
                }
            }

            return true;
        }

        /// <summary>
        /// Indicates if joint values have changed in model.
        /// </summary>
        /// <returns>True if joint values have changed.</returns>
        private bool JointTargetsHaveChanged()
        {
            for (int i = 0; i < this.currentJointTargets.Length; ++i)
            {
                if (Math.Abs(this.currentJointTargets[i] - Model.JointCommandedPosition[i]) > MathConstants.ErrorEpsilon)
                {
                    return true;
                }
            }

            return false;
        }
    }
}
